#include "aresmotecdrive.h"

AresMotecDrive::AresMotecDrive( QObject* parent ) {
    time = 50;

#ifdef NJLAKCAN
    canDevice = new NJLikeCanDevice();
#endif
#ifdef ZLGCAN
    canDevice = new ZLGCanDevice();
#endif
    //舵轮转角参数-ARES
    Zero_Pulse                       = 0;
    pulse_per                        = 131072;  //电机一圈脉冲数值
    Veer_Wheel_Speed_Reduction_Radio = 320;     // 减速比 40 × 齿轮比 8（144/18）
    pos_mode_speed                   = 3000;    //舵轮转角 0~5000 rpm
    pos_mode_acc                     = 900;     // 1~1000 rps^2
    pos_mode_dec                     = 900;     // 1~1000 rps^2
}

bool AresMotecDrive::CanRecviveData() {
    if ( !is_open_can ) { return false; }
    CanDataFrame vco[ 200 ];
    uint         dwRel = static_cast< uint >( canDevice->CanChannelReceive( dw_DeviceHandle, CAN_DEVICE_TYPE, dw_Index, dw_Channel, vco, 200, 40 ) );
    if ( dwRel <= 0 ) { return false; }
    for ( ULONG i = 0; i < dwRel; i++ ) {
        QByteArray recv;
        for ( int k = 0; k < vco[ i ].nDataLen; k++ ) {
            recv.append( static_cast< char >( vco[ i ].arryData[ k ] ) );
        }
        emit sigShowCanData( recv, "Recv1:" + QString::number( vco[ i ].uID, 16 ) );  //发送打印static_cast<short int>
        // 直行反馈速度
        if ( vco[ i ].uID == 0X1A6 ) {
            // rpm
            short int recive_walk_speed = static_cast< short int >( ( vco[ i ].arryData[ 0 ] ) + ( vco[ i ].arryData[ 1 ] << 8 ) );
            // m/s
            fork_walk_reality_speed = static_cast< double >( recive_walk_speed ) / Walk_Wheel_Speed_Reduction_Radio / 60 * ( 2 * M_PI * Walk_Wheel_R );
            //            qDebug()<<"舵轮直行反馈真实速度："<<fork_walk_reality_speed<<endl;
        }
        // ARES 舵轮脉冲反馈
        else if ( vco[ i ].uID == 0x181 ) {
            ARES_status_word      = vco[ i ].arryData[ 0 ] + ( vco[ i ].arryData[ 1 ] << 8 );
            int value             = vco[ i ].arryData[ 4 ] + ( vco[ i ].arryData[ 5 ] << 8 ) + ( vco[ i ].arryData[ 6 ] << 16 ) + ( vco[ i ].arryData[ 7 ] << 24 );
            fork_veer_reality_rad = static_cast< double >( value - Zero_Pulse ) / pulse_per / Veer_Wheel_Speed_Reduction_Radio * ( 2 * M_PI );
        }
        // ARES AUTO CHANGE
        else if ( vco[ i ].uID == 0x381 ) {
            static bool flag = false;
            if ( ( vco[ i ].arryData[ 0 ] & 0b1 ) == 0b0 ) {  // 拨到了自动
                if ( !flag /*((ARES_status_word >> 1) & 0b1) == 0b0*/ ) {
                    helm_init();
                    flag = true;
                }
            }
            else {  // 拨到了手动
                flag = false;
            }
        }
    }
    return true;
}

void AresMotecDrive::RegularGetData() {
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];
    CanDataFrameSetForkRaiseSpeed( frames[ i++ ], fork_lft_speed );
    CanDataFrameSetKDSData( frames[ i++ ], kds_speed, kds_rise, kds_fall );
    /***********ARES Angle Set*************/
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x0F;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = ( BYTE )( fork_calc_puls & 0xFF );
    frames[ i ].arryData[ 3 ] = ( BYTE )( ( fork_calc_puls >> 8 ) & 0xFF );
    frames[ i ].arryData[ 4 ] = ( BYTE )( ( fork_calc_puls >> 16 ) & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( fork_calc_puls >> 24 ) & 0xFF );
    i++;
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x5F;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = ( BYTE )( fork_calc_puls & 0xFF );
    frames[ i ].arryData[ 3 ] = ( BYTE )( ( fork_calc_puls >> 8 ) & 0xFF );
    frames[ i ].arryData[ 4 ] = ( BYTE )( ( fork_calc_puls >> 16 ) & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( fork_calc_puls >> 24 ) & 0xFF );
    i++;

    CanSendData( frames, i );
}

void AresMotecDrive::CanSetSpeedAngle( double walk_speed, double veer_rad ) {
    fork_walk_setting_speed = walk_speed;
    fork_veer_setting_rad   = veer_rad;
    // 速度m/s = 给定速度（Walk_Wheel_Speed）* 60 / 周长（Walk_Wheel_C） *
    // 减速比（Walk_Wheel_Speed_Reduction_Radio）
    kds_speed = static_cast< short >( ( fork_walk_setting_speed * 60 / Walk_Wheel_C ) * Walk_Wheel_Speed_Reduction_Radio );
    // ARES 脉冲值 = 弧度
    fork_calc_puls = static_cast< long >( fork_veer_setting_rad * Veer_Wheel_Speed_Reduction_Radio * pulse_per / ( 2 * M_PI ) + Zero_Pulse );
}

void AresMotecDrive::CanDataFrameSetKDSData( CanDataFrame& frame, short& speed, uchar& kdsRise, uchar& kdsFall ) {
    BYTE sendbuffer[ 8 ];
    sendbuffer[ 0 ]   = static_cast< BYTE >( speed );
    sendbuffer[ 1 ]   = static_cast< BYTE >( speed >> 8 );
    sendbuffer[ 2 ]   = 0;
    sendbuffer[ 3 ]   = 0;
    sendbuffer[ 4 ]   = kdsRise;
    sendbuffer[ 5 ]   = kdsFall;
    sendbuffer[ 6 ]   = 0;
    sendbuffer[ 7 ]   = 0;
    frame.uID         = 0x226;
    frame.nSendType   = 0;
    frame.bRemoteFlag = 0;
    frame.bExternFlag = 0;
    frame.nDataLen    = 8;
    for ( int i = 0; i < 8; i++ ) {
        frame.arryData[ i ] = sendbuffer[ i ];
    }
}

void AresMotecDrive::CanDataFrameSetForkRaiseSpeed( CanDataFrame& frame, uchar value ) {
    frame.uID           = 0x222;
    frame.nSendType     = 0;
    frame.bRemoteFlag   = 0;
    frame.bExternFlag   = 0;
    frame.nDataLen      = 1;
    frame.arryData[ 0 ] = value;
}

void AresMotecDrive::helm_init() {
    // ARES_init
    CanDataFrame frames[ 20 ];
    ULONG        i = 0;
    // T曲线
    
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x86;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = 0x00;
    frames[ i ].arryData[ 5 ] = 0x00;
    i++;
    // 最大速度
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x7F;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pos_mode_speed & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pos_mode_speed >> 8 ) & 0xFF );
    // 编码器分辨率
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 8;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x23;
    frames[ i ].arryData[ 1 ] = 0x8F;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pulse_per & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pulse_per >> 8 ) & 0xFF );
    frames[ i ].arryData[ 6 ] = ( BYTE )( ( pulse_per >> 16 ) & 0xFF );
    frames[ i ].arryData[ 7 ] = ( BYTE )( ( pulse_per >> 24 ) & 0xFF );
    i++;
    // 最大加速度
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x82;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pos_mode_acc & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pos_mode_acc >> 8 ) & 0xFF );
    i++;
    // 最大减速度
    frames[ i ].uID           = 0x601;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x2B;
    frames[ i ].arryData[ 1 ] = 0x83;
    frames[ i ].arryData[ 2 ] = 0x60;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = ( BYTE )( pos_mode_dec & 0xFF );
    frames[ i ].arryData[ 5 ] = ( BYTE )( ( pos_mode_dec >> 8 ) & 0xFF );
    i++;
    //发送06电机通电
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x06;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = 0x00;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = 0x00;
    frames[ i ].arryData[ 5 ] = 0x00;
    i++;
    //发送07电机使能
    frames[ i ].uID           = 0x201;
    frames[ i ].nDataLen      = 6;
    frames[ i ].nSendType     = 0;
    frames[ i ].bRemoteFlag   = 0;
    frames[ i ].bExternFlag   = 0;
    frames[ i ].arryData[ 0 ] = 0x07;
    frames[ i ].arryData[ 1 ] = 0x00;
    frames[ i ].arryData[ 2 ] = 0x00;
    frames[ i ].arryData[ 3 ] = 0x00;
    frames[ i ].arryData[ 4 ] = 0x00;
    frames[ i ].arryData[ 5 ] = 0x00;
    i++;
    CanSendData( frames, i );
    emit sigSendTips( "ARES转角电机初始化" );
}

void AresMotecDrive::sltForkLiftDrive_Init() {
    if ( CanOpenAndStart() ) {  // open can drive
        emit sigSendTips( "aresMotecDrive 正确，CAN1设备打开成功" );
    }
    else {
        emit sigSendTips( "aresMotecDrive 错误，CAN1设备打开失败" );
        return;
    }
    ULONG        i = 0;
    CanDataFrame frames[ 20 ];
    helm_init();

    /******行走轮设定速度*******/
    CanDataFrameSetKDSData( frames[ i++ ], kds_speed, kds_rise, kds_fall );
    CanSendData( frames, i );
    emit sigSendTips( "提示，驱动电机初始化完成！" );
    Utility::delayMs( 1000 );  // 非阻塞延时执行
    QTimer* timer50ms = new QTimer();
    connect( timer50ms, SIGNAL( timeout() ), this, SLOT( sltTimeOut() ) );
    timer50ms->start( time );
}
